package pl.elka.simulator.core.mobilityManager.impl;

import java.util.ArrayList;
import java.util.List;

import pl.elka.simulator.core.AreaConstraint;
import pl.elka.simulator.core.CollisionSolver;
import pl.elka.simulator.core.Constraint;
import pl.elka.simulator.core.EnviromentConfiguration;
import pl.elka.simulator.core.IPaintable;
import pl.elka.simulator.core.Node;
import pl.elka.simulator.core.Position;
import pl.elka.simulator.core.Segment;
import pl.elka.simulator.core.configurationCommand.ConfigurationCommandType;
import pl.elka.simulator.core.experiment.ConsistencyRaitingCounter;
import pl.elka.simulator.core.mobilityManager.IMobilityManager;
import pl.elka.simulator.core.mobilityManager.MobilityManagerType;
import pl.elka.simulator.core.mobilityManager.impl.nodes.ExplorerNode;
import pl.elka.simulator.core.mobilityManager.impl.nodes.RWMNode;

public class ExplorerMobilityManager implements IMobilityManager{

	private static final float AVOID_RADUNDATION_PRIORITY = 1;
	private static final float FOLLOW_NEIGHBOURS_PRIORITY = 1f;
	private static final float AVOID_COLLISION_PRIORITY = 4f;
	
	private final float BEST_NEIGHBOUR_DISTANCE = ConsistencyRaitingCounter.NEIGHBOUR_DISTANCE_SQRT/4;//polowa zasiegu
	private String id;
	
	private Node[] localGroupNodes;
	protected List<Constraint> constraints;
	protected AreaConstraint areaConstraint;
	protected CollisionSolver collisionSolver;
	
	protected List<ExplorerNode> nodes;
	
	@Override
	public IMobilityManager newInstance() {	
		return new ExplorerMobilityManager();
	}

	@Override
	public MobilityManagerType getType() {
		return MobilityManagerType.Explorer;
	}

	@Override
	public List<ConfigurationCommandType> getAllowedConfigurationCommands() {
		List<ConfigurationCommandType> ret = new ArrayList<ConfigurationCommandType>();		
		return ret;
	}
	@Override
	public void setId(String id) {		
		this.id = id;
	}

	@Override
	public String getId() {
		return id;
	}

	@Override
	public void makeMove(long elapsedTime) {
		for(ExplorerNode n: nodes){
			//shareSniffingData(n);
			if(n.isSinffing){
				keepSniffing(n,elapsedTime);
			}else{
				moveNode(n,elapsedTime);
			}
			n.path.addPathElement(n.position.clone());
		}
	}
	

	private void keepSniffing(ExplorerNode n, long elapsedTime) {
		n.velocity = n.sniffingDirection;
		Position newPosition = new Position(
				n.position.x + n.sniffingDirection.x*elapsedTime,
				n.position.y + n.sniffingDirection.y*elapsedTime
			);
	
		Position collisionPosition = collisionSolver.findCollisionPosition(n.position, newPosition);
		if(collisionPosition==null){
			n.position = newPosition;
			n.sniffingTime--;
			if(n.sniffingTime<=0){
				n.isSinffing = false;
			}
		}else{
			Position redundationForce = countAvoidRedundationForce(n);	
			redundationForce.add(n.sniffingDirection);
			startSniffing(n,elapsedTime,redundationForce,collisionPosition,newPosition);
		}
	}
	private Position randomVelocity(){
		float xv =  (float)(Math.random()-0.5);
		float yv =  (float)(Math.random()-0.5);
		Position ret =  new Position(xv,yv);
		ret.setMagnitude(Node.MAX_VELOCITY);
		return ret;		
	}
	private void moveNode(ExplorerNode n,long elapsedTime) {			
		Position redundationForce = countAvoidRedundationForce(n);		
		Position followForce = countFollowForce(n);
		Position avoidForce = avoidNearest(n);
		
		Position v = null;
		if(redundationForce.isZero() && followForce.isZero() && avoidForce.isZero()){
			v = randomVelocity();
		}else{
			v = redundationForce;
			v.add(followForce);		
			v.add(avoidForce);
			v.setMagnitude(Node.MAX_VELOCITY);
		}
		
		Position newPosition = new Position(
				n.position.x + v.x*elapsedTime,
				n.position.y + v.y*elapsedTime
			);
	
		Position collisionPosition = collisionSolver.findCollisionPosition(n.position, newPosition);
		if(collisionPosition==null){
			n.position = newPosition;			
		}else{
			try{
				startSniffing(n,elapsedTime,v,collisionPosition,newPosition);
			}catch(NullPointerException e)
			{
				e.printStackTrace();
			}
			//n.position = collisionPosition;
		}
		n.velocity = v;
				
	}
	
	private Position avoidNearest(ExplorerNode node) {
		
		
		Node nearestNode = null;
		float nearestNodeDistanceSqrt = Float.MAX_VALUE;
		for(Node n:nodes){
			if(n!=node){
				float distSqrt = node.position.distSqrt(n.position);
				if(distSqrt<=ConsistencyRaitingCounter.NEIGHBOUR_DISTANCE_SQRT){					
					if(distSqrt<nearestNodeDistanceSqrt){
						nearestNodeDistanceSqrt = distSqrt;
						nearestNode = n;
					}
				}
			}
		}
		if(nearestNode == null || nearestNodeDistanceSqrt>=BEST_NEIGHBOUR_DISTANCE)
			return new Position();
					
		Position v = node.position.minusNew(nearestNode.position);
		v.setMagnitude(ConsistencyRaitingCounter.NEIGHBOUR_DISTANCE/2);
		v.mul(AVOID_COLLISION_PRIORITY);
		return v;
	}

	private void startSniffing(ExplorerNode n, long elapsedTime, Position v,Position collisionPosition,Position newPosition) {				
		Segment collisionSegment = collisionSolver.findCollisionSegment(n.position, newPosition);
			
		// odsuwam od przeszkody na 1 cm
		Position moveVec = n.position.minusNew(collisionPosition);
		moveVec.setMagnitude(0.005f);					
		collisionPosition.add(moveVec);
		n.position = collisionPosition;
		
		Position target = null;
		Position beginNorm = collisionSegment.begin.minusNew(collisionPosition).setMagnitude(1);
		Position endNorm = collisionSegment.end.minusNew(collisionPosition).setMagnitude(1);
		float dstFromSegmentBegin = beginNorm.minusNew(v).countLenSqrt();
		float dstFromSegmentEnd = endNorm.minusNew(v).countLenSqrt();
		
		if(dstFromSegmentBegin<dstFromSegmentEnd){
			target = collisionSegment.begin;			
		}else{
			target = collisionSegment.end;
		}
		if(isSniffed(n,collisionSegment,target)){
			//jesli ten segment jest juz przez kogos badany lub juz byl badany, to ignaruje
			n.isSinffing = false;
			return;
		}
		Position tTarget = target.addNew(moveVec);
		tTarget.minus(n.position);//kierunek ruchu
		float dstToTarget = tTarget.countLen();
		tTarget.setMagnitude(Node.MAX_VELOCITY);
		
		n.sniffingTime = (int)Math.ceil(dstToTarget/Node.MAX_VELOCITY);		
		n.sniffingDirection = tTarget;
		n.isSinffing = true;
		n.sniffedSegment = collisionSegment;
		n.sniffedTarget = target;
		
		n.addSniffedSegment(collisionSegment, target);
	}

	/**
	 * czy ktos bada ten segment w tym kierunku
	 * @param node
	 * @param collisionSegment
	 * @param target
	 * @return
	 */
	private boolean isSniffed(ExplorerNode node, Segment collisionSegment,	Position target) {
		float distSqrt;
		for(ExplorerNode n:nodes){
			distSqrt = node.position.distSqrt(n.position);
			if( distSqrt <= ConsistencyRaitingCounter.NEIGHBOUR_DISTANCE_SQRT ){
				if(n!=node && n.isSinffing){
					if(n.sniffedSegment == collisionSegment && n.sniffedTarget.equals(target)){
						return true;
					}
				}
				if(n.contains(collisionSegment, target)){
					return true;
				}
			}
		}
		return false;
	}

	private boolean shareSniffingData(ExplorerNode node) {		
		for(ExplorerNode n:nodes){
			if(n == node) continue;			
			if( node.position.distSqrt(n.position) <= ConsistencyRaitingCounter.NEIGHBOUR_DISTANCE_SQRT ){
				node.sniffedSegments.addAll(n.sniffedSegments);
			}
		}
		return false;
	}
	
	private Position countFollowForce(ExplorerNode node) {			
		if(nodes.size() == 1){
			return new Position();
		}
				
		float distSqrt = 0;
		int localCounter = 0;
		Position middlePoint = new Position();
		Position avgVel = new Position();
		for(ExplorerNode n:nodes){
			distSqrt = node.position.distSqrt(n.position);
			if( distSqrt <= ConsistencyRaitingCounter.NEIGHBOUR_DISTANCE_SQRT ){
				middlePoint.add(n.position);	
				avgVel.add(n.velocity);
				++localCounter;
			}
		}
		middlePoint.div(localCounter);
		middlePoint.minus(node.position);
		avgVel.div(localCounter);
		
		middlePoint.setMagnitude(FOLLOW_NEIGHBOURS_PRIORITY);
		avgVel.setMagnitude(FOLLOW_NEIGHBOURS_PRIORITY);
		
		middlePoint.add(avgVel);
				
		return middlePoint;
	}

	private Position countAvoidRedundationForce(Node n) {
		Position redundationForce = new Position(0,0);
		for(Position p : n.path.path){
			float distSqrt = n.position.distSqrt(p);
			if(distSqrt == 0) continue;
			Position dif = n.position.minusNew(p);
			dif.setMagnitude(1/distSqrt);
			redundationForce.add(dif);
		}
		redundationForce.setMagnitude(AVOID_RADUNDATION_PRIORITY);
		return redundationForce;
	}

	@Override
	public List<IPaintable> getPaintaibleElements() {
		List<IPaintable> ret = new ArrayList<IPaintable>();		
		if(nodes!=null){
			for(Node n:nodes){
				ret.add(n.getPath());
			}
		}
		return ret;
	}

	@Override
	public void applyEnviroment(EnviromentConfiguration cfg) {
		nodes  = new ArrayList<ExplorerNode>();
		
		for(Node n:cfg.getNodes()){
			ExplorerNode newNode = new ExplorerNode(n);
			newNode.path.addPathElement(n.position.clone());
			nodes.add(newNode);						
		}
		
		constraints = cfg.getConstraints();
		areaConstraint = cfg.getAreaConstraint();		
		collisionSolver = new CollisionSolver(cfg);
		localGroupNodes = new ExplorerNode[nodes.size()];
	}

	@Override
	public List<? extends Node> getNodes() {
		return nodes;
	}
	
	@Override
	public void clear() {
		nodes = null;
	}
	
}
